#include "Point.h"
#include "Matrix.h"
#include <valarray>
#include <vector>
#include <iostream>
#include <cstdlib>
#include <fstream>

#define TEMPLATE template<int DIM, typename T>
#define TEMPLATE2D template <typename T>
using std::valarray;

TEMPLATE class Element;

TEMPLATE2D class TriElement;

TEMPLATE class Element
{
protected:
    valarray<Point<DIM, T> > local_dofs;  //参考坐标下的点
    valarray<Point<DIM, T> > global_dofs; //全局点
    valarray<Point<DIM, T> > quad_points; // 计算积分时的点编号
    valarray<T> quad_weights; //计算积分时的点权重
    int algebraic_accuracy; //积分的精度
public:
    Element(){algebraic_accuracy = 0; };
    void set_n_dofs(size_t _n);
    size_t get_n_dofs() const;
    void set_local_dof(size_t _i, const Point<DIM, T> &_pnt);
    void set_global_dof(size_t _l, const Point<DIM,T> &_pnt);
    const Point<DIM, T> & get_local_dof(size_t _i) const;
    const Point<DIM, T> & get_global_dof(size_t _i) const;
    const valarray<Point<DIM,T> > & get_local_dofs() const;
    const valarray<Point<DIM,T> > & get_global_dofs() const; 
    size_t get_n_quad_points();
    const valarray<Point<DIM,T> >& get_quad_points() const;
    const valarray<T> &get_quad_weights() const;
    size_t get_algebraic_accuracy() const;
    virtual T lambda_function(size_t _i, const Point<DIM, T> &_pnt) const = 0;//参考坐标下的_i基函数在_pnt处的值
    virtual valarray<T> lambda_gradient(size_t _i, const Point<DIM, T> &_pnt) const = 0;//参考坐标下_i基函数在_pnt的梯度
    virtual Point<DIM, T> local2global(const Point<DIM, T> &_pnt) const = 0;//将局部坐标点变成全局坐标
    virtual T basis_function(size_t _i, const Point<DIM,T> &_pnt) const = 0;//在点_pnt处，全局基函数_i的取值
    virtual valarray<T> basis_gradient(size_t _i, const Point<DIM, T> &_pnt) const = 0;//在点_pnt处，全局基函数_i的梯度
};

TEMPLATE void Element<DIM, T>::set_n_dofs(size_t _n)
{
    local_dofs.resize(_n);
    global_dofs.resize(_n);
};

TEMPLATE size_t Element<DIM, T>::get_n_dofs() const
{
    size_t n_local_dofs = local_dofs.size();
    size_t n_global_dofs = global_dofs.size();
    if(n_local_dofs != n_global_dofs)
    {
        std::cerr <<"No. of dofs error." << std::endl;
        std::exit(-1);
    }
    return n_local_dofs;
};

TEMPLATE size_t Element<DIM, T> ::get_n_quad_points()
{
    size_t n_q_pnts = quad_points.size();
    size_t n_q_wei = quad_weights.size();
    if(n_q_pnts != n_q_wei)
    {
        std::cerr <<"No. of quad points error." << std::endl;
        std::exit(-1);
    }
    return n_q_pnts;
};

TEMPLATE size_t Element<DIM, T>:: get_algebraic_accuracy() const
{
    return algebraic_accuracy;
};

TEMPLATE void Element<DIM, T>::set_global_dof(size_t _i, const Point<DIM,T> &_pnt)
{
    global_dofs[_i] = _pnt;
    global_dofs[_i].set_index(_pnt.get_index());
};

TEMPLATE const Point<DIM, T>& Element<DIM,T>::get_global_dof(size_t _i) const { return global_dofs[_i];};

TEMPLATE const Point<DIM, T>& Element<DIM,T>::get_local_dof(size_t _i) const { return local_dofs[_i];};

TEMPLATE const valarray<Point<DIM, T> >& Element<DIM,T>::get_local_dofs() const
{
    return local_dofs;
};

TEMPLATE const valarray<Point<DIM, T> >& Element<DIM,T>::get_global_dofs() const
{
    return global_dofs;
};

TEMPLATE const valarray<Point<DIM, T> >& Element<DIM,T>::get_quad_points() const
{
    return quad_points;
};

TEMPLATE const valarray<T>& Element<DIM,T>::get_quad_weights() const
{
    return quad_weights;
};

TEMPLATE void Element<DIM, T>::set_local_dof(size_t _i, const Point<DIM,T> &_pnt)
{
    local_dofs[_i] = _pnt;
    local_dofs[_i].set_index(_i);
};

TEMPLATE2D class TriElement : public Element<2,T>
{
private:
    size_t n_dofs = 3;
public:
    TriElement();
    TriElement(int _n_dofs);
    void read_quad_info(const char* filename, size_t _acc);
    T lambda_function(size_t _i, const Point<2,T> &_pnt) const;
    valarray<T> lambda_gradient(size_t _i, const Point<2,T> &_pnt) const;
    Point<2,T> local2global(const Point<2,T> &_pnt) const;
    Point<2,T> global2local(const Point<2,T> &_pnt) const;
    T global2local_Jacobi_det(const Point<2,T> &_pnt) const;
    Matrix<T> global2local_Jacobi(const Point<2,T> &_pnt) const;
    Matrix<T> local2global_Jacobi(const Point<2,T> &_pnt) const;
    T get_volume() const;
    Point<2,T> lambda2local(std::vector<T> _lambda) const;
    // T local2lambda(size_t _i, const Point<2,T> &_pnt) const;
};

TEMPLATE2D TriElement<T>::TriElement() : Element<2,T>()
{
    Element<2,T>::set_n_dofs(n_dofs);
    Point<2,T> pnt({0.0,0.0});
    Element<2,T>::set_local_dof(0, pnt);
    pnt ={1.0, 0.0};
    Element<2,T>::set_local_dof(1,pnt);
    pnt ={0.0,1.0};
    Element<2,T>::set_local_dof(2, pnt);
};

TEMPLATE2D TriElement<T>::TriElement(int _n_dofs) : Element<2,T>()
{
    if(_n_dofs < 3)
        n_dofs = 3;
    else
        n_dofs = _n_dofs;
    Element<2,T>::set_n_dofs(n_dofs);
    Point<2,T> pnt({0.0,0.0});
    Element<2,T>::set_local_dof(0, pnt);
    pnt ={1.0, 0.0};
    Element<2,T>::set_local_dof(1,pnt);
    pnt ={0.0,1.0};
    Element<2,T>::set_local_dof(2, pnt);
};

TEMPLATE2D T TriElement<T>::lambda_function(size_t _i, const Point<2,T> &_pnt) const 
{
    double xi = _pnt[0];
    double eta = _pnt[1];
    switch (_i)
    {
    case 0:
        return (1- xi - eta);
    case 1:
        return xi;
    case 2:
        return eta;
    }
};
/**
 * @brief 输入一个单元内的参考坐标把它还原成全局坐标，这个待重写。
 * 
 * @param _pnt 
 * @return TEMPLATE2D 
 */
TEMPLATE2D Point<2,T> TriElement<T>::local2global(const Point<2,T> &_pnt) const
{
    Point<2, T> global_pnt({0.0, 0.0});
    const valarray<Point<2, T> > &gdofs = Element<2, T>::get_global_dofs();
    for(size_t i = 0; i< 3; i++)
        global_pnt += gdofs[i] * lambda_function(i, _pnt);
    return global_pnt;
};

TEMPLATE2D valarray<T> TriElement<T>::lambda_gradient(size_t _i, const Point<2, T> &_pnt) const
{
    switch (_i)
    {
        case 0:
            return valarray<T> ({-1.0, -1.0});
        case 1:
            return valarray<T> ({1.0, 0.0});
        case 2:
            return valarray<T> ({0.0, 1.0});
    }
};

TEMPLATE2D Point<2,T> TriElement<T>::global2local(const Point<2,T> &_pnt) const
{
    valarray<T> global({_pnt[0]-(this->global_dofs)[0][0], _pnt[1]-(this->global_dofs)[0][1]});
    valarray<T> local = local2global_Jacobi(_pnt).transpose()*(global);
    Point<2,T> local_pnt({local[0], local[1]});
    return local_pnt;
};

TEMPLATE2D
Matrix<T> TriElement<T>::global2local_Jacobi(const Point<2,T> &_pnt) const // d(x,y)/d(xi,eta)
{
    const valarray<Point<2,T> > &gdofs = Element<2,T>::get_global_dofs();
    Matrix<T> res(2,2);
    res(0,0) = gdofs[1][0] - gdofs[0][0];
    res(0,1) = gdofs[1][1] - gdofs[0][1];
    res(1,0) = gdofs[2][0] - gdofs[0][0];
    res(1,1) = gdofs[2][1] - gdofs[0][1];
    return res;
};

TEMPLATE2D
T TriElement<T>::global2local_Jacobi_det(const Point<2,T> &_pnt) const 
{
    Matrix<T> J = global2local_Jacobi(_pnt);
    return (J(0,0)*J(1,1)-J(0,1)*J(1,0));
};

TEMPLATE2D T TriElement<T>::get_volume() const
{
    const Point<2,T> &pnt0 = Element<2,T>::local_dofs[0];
    const Point<2,T> &pnt1 = Element<2,T>::local_dofs[1];
    const Point<2,T> &pnt2 = Element<2,T>::local_dofs[2];
    return 0.5*((pnt1[0]-pnt0[0])*(pnt2[1]-pnt0[1])-(pnt1[1]-pnt0[1])*(pnt2[0]-pnt0[0]));
};

TEMPLATE2D Matrix<T> TriElement<T>::local2global_Jacobi(const Point<2,T> &_pnt) const // d(xi,eta)/d(x,y)
{
    Matrix<T> res(2,2);
    Matrix<T> J = global2local_Jacobi(_pnt);
    T det = J(0,0)*J(1,1)-J(0,1)*J(1,0);
    res(0,0) = J(1,1);
    res(1,0) = -J(0,1);
    res(0,1) = -J(1,0);
    res(1,1) = J(0,0);
    res /= det;
    return res;
};

TEMPLATE2D void TriElement<T>::read_quad_info(const char* filename, size_t _acc)
{
    std::cout << "Reading quadrature data ..." << std::endl;
    std::ifstream is(filename);
    char text[64];
    size_t n_tb1 , n_qpoints, acc, next_acc;
    is >> n_tb1;
    if(n_tb1 < _acc)
    {
        std::cerr << "The required algebraic accuracy "<< _acc
            << "is too high," <<"and the actual algebraic accuracy is"
            << n_tb1 << std::endl;
        acc = n_tb1;
    }
        else acc = _acc;
    while(1)
    {
        is >> next_acc;
        if( next_acc >= acc)
        {
            is >> n_qpoints;
            Element<2,T>::quad_points.resize(n_qpoints);
            Element<2,T>::quad_weights.resize(n_qpoints);
            for(size_t i = 0; i < n_qpoints; i++)
            {
                T x ,y, w;
                is >> x >> y >> w;
                Element<2,T>::quad_points[i][0] = x;
                Element<2,T>::quad_points[i][1] = y;
                Element<2,T>::quad_weights = w;
            }
            Element<2,T>::algebraic_accuracy = acc;
            break;
        }
        else
        {
            is >> n_qpoints;
            for( size_t i =0; i < n_qpoints;i++)
                is.getline(text, 64);
            is.getline(text, 64);
        }
    }
    std::cout << " OK! " << std::endl;
};

TEMPLATE2D Point<2,T> TriElement<T>::lambda2local( std::vector<T> _lambda) const
{
    Point<2,T> res({_lambda[1], _lambda[2]});
    return res;
};
// TEMPLATE2D T TriElement<T>::local2lambda(size_t _i, const Point<2,T> &_pnt) const
// {
//     double eta = _pnt[0];
//     double xi = _pnt[1];
//     switch(_i)
//     {
//         case 0: return (1-xi-eta) ;
//         case 1: return xi ;
//         case 2: return eta ;
//     }
// };

TEMPLATE2D class P1Element : public TriElement<T>
{
public:
    P1Element(){};
    T basis_function(size_t _i, const Point<2,T> &_pnt) const;
    valarray<T> basis_gradient(size_t _i, const Point<2,T> &_pnt) const;
};

TEMPLATE2D T P1Element<T>::basis_function(size_t _i, const Point<2,T> &_pnt) const
{
    double xi = _pnt[0];
    double eta = _pnt[1];
    switch(_i)
    {
        case 0: return (1-xi-eta) ;
        case 1: return xi ;
        case 2: return eta ;
    }
};

TEMPLATE2D valarray<T> P1Element<T>::basis_gradient(size_t _i, const Point<2,T> &_pnt) const
{
    switch(_i)
    {
        case 0: return valarray<T>({-1.0, -1.0});
        case 1: return valarray<T>({1.0 , 0.0});
        case 2: return valarray<T>({0.0 , 1.0});
    }
};

TEMPLATE2D class P2Element : public TriElement<T>
{
public:
    P2Element();
    T basis_function(size_t _i, const Point<2,T> &_pnt) const;
    valarray<T> basis_gradient(size_t _i, const Point<2,T> &_pnt) const;
};

TEMPLATE2D P2Element<T>::P2Element() : TriElement<T>(6)
{
    Point<2,T> pnt({0.5, 0.5});
    Element<2,T>::set_local_dof(3,pnt);
    pnt = {0.0 , 0.5};
    Element<2,T>::set_local_dof(4,pnt);
    pnt = {0.5 , 0.0};
    Element<2,T>::set_local_dof(5,pnt);
};

TEMPLATE2D T P2Element<T>::basis_function(size_t _i, const Point<2,T> &_pnt) const
{
    double xi = _pnt[0];
    double eta = _pnt[1];
    switch(_i)
    {
        case 0: return (1-xi-eta)*(1.0 - 2.0 *xi -2.0 *eta);
        case 1: return xi*(2.0 *xi -1.0);
        case 2: return eta*(2.0 *eta - 1.0);
        case 3: return 4.0* xi* eta;
        case 4: return 4.0 *(1.0- xi- eta)*eta;
        case 5: return 4.0 *(1.0- xi- eta)*xi;
    }
};

TEMPLATE2D valarray<T> P2Element<T>::basis_gradient(size_t _i, const Point<2,T> &_pnt) const
{
    double xi = _pnt[0];
    double eta = _pnt[1];
    double phi_xi, phi_eta;
    switch(_i)
    {
        case 0:
            phi_xi = phi_eta = -3.0 +4.0 *xi + 4.0 *eta;
            return valarray<T>({phi_xi, phi_eta});
        case 1:
            phi_xi = 4.0* xi - 1.0;
            phi_eta = 0.0;
            return valarray<T>({phi_xi, phi_eta});
        case 2:
            phi_xi = 0.0;
            phi_eta = 4.0 *eta -1.0;
            return valarray<T>({phi_xi, phi_eta});
        case 3:
            phi_xi = 4.0 *eta;
            phi_eta = 4.0 * xi;
            return valarray<T>({phi_xi, phi_eta});
        case 4:
            phi_xi = -4.0 *eta;
            phi_eta = 4.0 - 4.0* xi -8.0*eta;
            return valarray<T>({phi_xi, phi_eta});
        case 5:
            phi_xi = 4.0 - 4.0*eta -8.0 *xi;
            phi_eta = - 4.0*xi;
            return valarray<T>({phi_xi, phi_eta});
    }
};

TEMPLATE2D class P3Element : public TriElement<T>
{
public:
    P3Element();
    T basis_function(size_t _i, const Point<2,T> &_pnt) const;
    valarray<T> basis_gradient(size_t _i, const Point<2,T> &_pnt) const;
};

TEMPLATE2D P3Element<T>::P3Element() : TriElement<T>(10)
{
    Point<2,T> pnt = TriElement<T>::lambda2local({1.0*2/3 , 1.0*1/3, 0.0});
    Element<2,T>::set_local_dof(3,pnt);
    pnt = TriElement<T>::lambda2local({1.0*1/3 , 1.0*2/3, 0.0});
    Element<2,T>::set_local_dof(4,pnt);
    pnt = TriElement<T>::lambda2local({0.0 , 1.0*2/3, 1.0*1/3});
    Element<2,T>::set_local_dof(5,pnt);
    pnt = TriElement<T>::lambda2local({0.0 , 1.0*1/3, 1.0*2/3});
    Element<2,T>::set_local_dof(6,pnt);
    pnt = TriElement<T>::lambda2local({1.0*1/3 , 0.0, 1.0*2/3});
    Element<2,T>::set_local_dof(7,pnt);
    pnt = TriElement<T>::lambda2local({1.0*2/3 , 0.0, 1.0*1/3});
    Element<2,T>::set_local_dof(8,pnt);
    pnt = TriElement<T>::lambda2local({1.0*1/3 , 1.0*2/3, 1.0*1/3});
    Element<2,T>::set_local_dof(9,pnt);
};

TEMPLATE2D T P3Element<T>::basis_function(size_t _i, const Point<2,T> &_pnt) const
{
    double xi = _pnt[0];
    double eta = _pnt[1];
    double lambda1 = 1 - xi - eta;
    double lambda2 = xi;
    double lambda3 = eta;
    switch(_i)
    {
        case 0: return lambda1* (3*lambda1 -1)*(3*lambda1 -2)/2;
        case 1: return lambda2* (3*lambda2 -1)*(3*lambda2 -2)/2;
        case 2: return lambda3* (3*lambda3 -1)*(3*lambda3 -2)/2;
        case 3: return 9.0 *lambda1 *lambda2 *(3*lambda1 -1) /2;
        case 4: return 9.0 *lambda1 *lambda2 *(3*lambda2 -1) /2;
        case 5: return 9.0 *lambda2 *lambda3 *(3*lambda2 -1) /2;
        case 6: return 9.0 *lambda2 *lambda3 *(3*lambda3 -1) /2;
        case 7: return 9.0 *lambda1 *lambda3 *(3*lambda3 -1) /2;
        case 8: return 9.0 *lambda1 *lambda3 *(3*lambda1 -1) /2;
        case 9: return 27.0 *lambda1 *lambda2 *lambda3;
    }
};

TEMPLATE2D valarray<T> P3Element<T>::basis_gradient(size_t _i, const Point<2,T> &_pnt) const
{
    double xi = _pnt[0];
    double eta = _pnt[1];
    double phi_xi, phi_eta;
    double phi_lambda1_xi = -1;
    double phi_lambda1_eta = -1;
    double phi_lambda2_eta = 0;
    double phi_lambda2_xi = 1;
    double phi_lambda3_eta = 1;
    double phi_lambda3_xi = 0;
    double lambda1 = 1- xi - eta;
    double lambda2 = xi;
    double lambda3 = eta;
    switch(_i)
    {
        case 0:
            phi_xi = phi_lambda1_xi *( (3.0*lambda1 -1.0)*(3.0 *lambda1-2.0)/2 +
                3.0*lambda1*(3.0*lambda1 -2.0)/2 + 3.0* lambda1*(3.0*lambda1 -1.0)/2 );
            phi_eta = phi_lambda1_eta *( (3.0*lambda1 -1.0)*(3.0 *lambda1-2.0)/2 +
                3.0*lambda1*(3.0*lambda1 -2.0)/2 + 3.0*lambda1 *(3.0*lambda1 -1.0)/2);
            return valarray<T>({phi_xi, phi_eta});
        case 1:
            phi_xi = phi_lambda1_xi *( (3.0*lambda2 -1.0)*(3.0 *lambda2-2.0)/2 +
                3.0*lambda2*(3.0*lambda2 -2.0)/2 + 3.0* lambda2*(3.0*lambda2 -1.0)/2 );
            phi_eta = phi_lambda1_eta *( (3.0*lambda2 -1.0)*(3.0 *lambda2-2.0)/2 +
                3.0*lambda2*(3.0*lambda2 -2.0)/2 + 3.0*lambda2 *(3.0*lambda2 -1.0)/2);
            return valarray<T>({phi_xi, phi_eta});
        case 2:
            phi_xi = phi_lambda3_xi *( (3.0*lambda3 -1.0)*(3.0 *lambda3-2.0)/2 +
                3.0*lambda3*(3.0*lambda3 -2.0)/2 + lambda3*(3.0*lambda3 -1.0)/2 );
            phi_eta = phi_lambda3_eta *( (3.0*lambda3 -1.0)*(3.0 *lambda3-2.0)/2 +
                3.0*lambda3*(3.0*lambda3 -2.0)/2 + lambda3*(3.0*lambda3 -1.0)/2 );
            return valarray<T>({phi_xi, phi_eta});
        case 3:
            phi_xi = 4.5 *( phi_lambda1_xi*(lambda2 *(3.0*lambda1 -1)) + phi_lambda2_xi *
                (lambda1 *(3.0*lambda1 -1)) + phi_lambda1_xi*3.0*lambda1*lambda2);
            phi_eta = 4.5 *( phi_lambda1_eta*(lambda2 *(3.0*lambda1 -1)) + phi_lambda2_eta *
                (lambda1 *(3.0*lambda1 -1)) + phi_lambda1_eta*3.0*lambda1*lambda2);
            return valarray<T>({phi_xi, phi_eta});
        case 4:
            phi_xi = 4.5 *( phi_lambda1_xi*(lambda2 *(3.0*lambda2 -1)) + phi_lambda2_xi *
                (lambda1 *(3.0*lambda2 -1)) + phi_lambda2_xi*3.0*lambda1*lambda2);
            phi_eta = 4.5 *( phi_lambda1_eta*(lambda2 *(3.0*lambda2 -1)) + phi_lambda2_eta *
                (lambda1 *(3.0*lambda2 -1)) + phi_lambda2_eta*3.0*lambda1*lambda2);
            return valarray<T>({phi_xi, phi_eta});
        case 5:
            phi_xi = 4.5 *( phi_lambda2_xi*(lambda3 *(3.0*lambda2 -1)) + phi_lambda3_xi *
                (lambda2 *(3.0*lambda2 -1)) + phi_lambda2_xi*3.0*lambda2*lambda3);
            phi_eta = 4.5 *( phi_lambda2_eta*(lambda3 *(3.0*lambda2 -1)) + phi_lambda3_eta *
                (lambda2 *(3.0*lambda2 -1)) + phi_lambda2_eta*3.0*lambda2*lambda3);
            return valarray<T>({phi_xi, phi_eta});
        case 6:
            phi_xi = 4.5 *( phi_lambda2_xi*(lambda3 *(3.0*lambda3 -1)) + phi_lambda3_xi *
                (lambda2 *(3.0*lambda3 -1)) + phi_lambda3_xi*3.0*lambda2*lambda3);
            phi_eta = 4.5 *( phi_lambda2_eta*(lambda3 *(3.0*lambda3 -1)) + phi_lambda3_eta *
                (lambda2 *(3.0*lambda3 -1)) + phi_lambda3_eta*3.0*lambda2*lambda3);
            return valarray<T>({phi_xi, phi_eta});
        case 7:
            phi_xi = 4.5 *( phi_lambda1_xi*(lambda3 *(3.0*lambda3 -1)) + phi_lambda3_xi *
                (lambda1 *(3.0*lambda3 -1)) + phi_lambda3_xi*3.0*lambda1*lambda3);
            phi_eta = 4.5 *( phi_lambda1_eta*(lambda3 *(3.0*lambda3 -1)) + phi_lambda3_eta *
                (lambda1 *(3.0*lambda3 -1)) + phi_lambda3_eta*3.0*lambda1*lambda3);
            return valarray<T>({phi_xi, phi_eta});
        case 8:
            phi_xi = 4.5 *( phi_lambda1_xi*(lambda3 *(3.0*lambda1 -1)) + phi_lambda3_xi *
                (lambda1 *(3.0*lambda1 -1)) + phi_lambda1_xi*3.0*lambda1*lambda3);
            phi_eta = 4.5 *( phi_lambda1_eta*(lambda3 *(3.0*lambda1 -1)) + phi_lambda3_eta *
                (lambda1 *(3.0*lambda1 -1)) + phi_lambda1_eta*3.0*lambda1*lambda3);
            return valarray<T>({phi_xi, phi_eta});
        case 9:
            phi_xi = 27.0 *(phi_lambda1_xi *lambda2 *lambda3 +phi_lambda2_xi *lambda1 *lambda3
                +phi_lambda3_xi *lambda1 *lambda2);
            phi_eta = 27.0 *(phi_lambda1_eta *lambda2 *lambda3 +phi_lambda2_eta *lambda1 *lambda3
                +phi_lambda3_eta *lambda1 *lambda2);
            return valarray<T>({phi_xi, phi_eta});
    }
};

TEMPLATE2D class P4Element : public TriElement<T>
{
public:
    P4Element();
    T basis_function(size_t _i, const Point<2,T> &_pnt) const;
    valarray<T> basis_gradient(size_t _i, const Point<2,T> &_pnt) const;
};

TEMPLATE2D P4Element<T>::P4Element() : TriElement<T>(15)
{
    Point<2,T> pnt = TriElement<T>::lambda2local({0.0 , 0.75, 0.25});
    Element<2,T>::set_local_dof(3,pnt);
    pnt = TriElement<T>::lambda2local({0.0 , 0.5, 0.5});
    Element<2,T>::set_local_dof(4,pnt);
    pnt = TriElement<T>::lambda2local({0.0 , 0.25, 0.75});
    Element<2,T>::set_local_dof(5,pnt);
    pnt = TriElement<T>::lambda2local({0.25 , 0.0, 0.75});
    Element<2,T>::set_local_dof(6,pnt);
    pnt = TriElement<T>::lambda2local({0.5 , 0.0, 0.5});
    Element<2,T>::set_local_dof(7,pnt);
    pnt = TriElement<T>::lambda2local({0.75 , 0.0, 0.25});
    Element<2,T>::set_local_dof(8,pnt);
    pnt = TriElement<T>::lambda2local({0.75 , 0.25, 0.0});
    Element<2,T>::set_local_dof(9,pnt);
    pnt = TriElement<T>::lambda2local({0.5 , 0.5, 0.0});
    Element<2,T>::set_local_dof(10,pnt);
    pnt = TriElement<T>::lambda2local({0.25 , 0.75, 0.0});
    Element<2,T>::set_local_dof(11,pnt);
    pnt = TriElement<T>::lambda2local({0.25 , 0.25, 0.5});
    Element<2,T>::set_local_dof(12,pnt);
    pnt = TriElement<T>::lambda2local({0.5 , 0.25, 0.25});
    Element<2,T>::set_local_dof(13,pnt);
    pnt = TriElement<T>::lambda2local({0.25 , 0.5, 0.25});
    Element<2,T>::set_local_dof(14,pnt);
};

TEMPLATE2D T P4Element<T>::basis_function(size_t _i, const Point<2,T> &_pnt) const
{
    double xi = _pnt[0];
    double eta = _pnt[1];
    double lambda1 = 1 - xi - eta;
    double lambda2 = xi;
    double lambda3 = eta;
    switch(_i)
    {
        case 0: return lambda1* (4*lambda1 -1)*(4*lambda1 -2)*(4*lambda1 -3)/6;//(4,0,0)
        case 1: return lambda2* (4*lambda2 -1)*(4*lambda2 -2)*(4*lambda2 -3)/6;//(0,4,0)
        case 2: return lambda3* (4*lambda3 -1)*(4*lambda3 -2)*(4*lambda3 -3)/6;//(0,0,4)
        case 3: return 8.0 *lambda3 *lambda2 *(4*lambda2 -1)*(4*lambda2-2) /3;//(0,3,1)
        case 4: return 4.0 *lambda2 *lambda3 *(4*lambda2 -1)*(4*lambda3 -1) ;//(0,2,2)
        case 5: return 8.0 *lambda3 *lambda2 *(4*lambda3 -1)*(4*lambda3-2) /3;//(0,1,3)
        case 6: return 8.0 *lambda1 *lambda3 *(4*lambda3 -1)*(4*lambda3-2) /3;//(1,0,3)
        case 7: return 4.0 *lambda1 *lambda3 *(4*lambda1 -1)*(4*lambda3 -1) ;//(2,0,2)
        case 8: return 8.0 *lambda1 *lambda3 *(4*lambda1 -1)*(4*lambda1-2) /3;//(3,0,1)
        case 9: return 8.0 *lambda1 *lambda2 *(4*lambda1 -1)*(4*lambda1-2) /3;//(3,1,0)
        case 10: return 4.0 *lambda1 *lambda2 *(4*lambda1 -1)*(4*lambda2 -1) ;//(2,2,0)
        case 11: return 8.0 *lambda1 *lambda2 *(4*lambda2 -1)*(4*lambda2-2) /3;//(1,3,0)
        case 12: return 32.0*lambda1*lambda2*lambda3 *(4*lambda3 -1);//(1,1,2)
        case 13: return 32.0*lambda1*lambda2*lambda3 *(4*lambda1 -1);//(2,1,1)
        case 14: return 32.0*lambda1*lambda2*lambda3 *(4*lambda2 -1);//(1,2,1)
    }
};

TEMPLATE2D valarray<T> P4Element<T>::basis_gradient(size_t _i, const Point<2,T> &_pnt) const
{
    double xi = _pnt[0];
    double eta = _pnt[1];
    double phi_xi, phi_eta;
    double phi_lambda1_xi = -1;
    double phi_lambda1_eta = -1;
    double phi_lambda2_xi = 1;
    double phi_lambda2_eta = 0;
    double phi_lambda3_eta = 1;
    double phi_lambda3_xi = 0;
    double lambda1 = 1- xi - eta;
    double lambda2 = xi;
    double lambda3 = eta;
    switch(_i)
    {
        case 0:
            phi_xi = phi_lambda1_xi*((4*lambda1 -1)*(4*lambda1-2)*(4*lambda1-3)+4*lambda1*(4*lambda1-2)*(4*lambda1 -3)
                + 4*lambda1*(4*lambda1-1)*(4*lambda1-3)+4*lambda1*(4*lambda1-1)*(4*lambda1 -2))/6;
            phi_eta = phi_lambda1_eta*((4*lambda1 -1)*(4*lambda1-2)*(4*lambda1-3)+4*lambda1*(4*lambda1-2)*(4*lambda1 -3)
                + 4*lambda1*(4*lambda1-1)*(4*lambda1-3)+4*lambda1*(4*lambda1-1)*(4*lambda1 -2))/6;
            return valarray<T>({phi_xi, phi_eta});
        case 1:
            phi_xi = phi_lambda2_xi*((4*lambda2 -1)*(4*lambda2-2)*(4*lambda2-3)+4*lambda2*(4*lambda2-2)*(4*lambda2 -3)
                + 4*lambda2*(4*lambda2-1)*(4*lambda2-3)+4*lambda2*(4*lambda2-1)*(4*lambda2 -2))/6;
            phi_eta = phi_lambda2_eta*((4*lambda2 -1)*(4*lambda2-2)*(4*lambda2-3)+4*lambda2*(4*lambda2-2)*(4*lambda2 -3)
                + 4*lambda2*(4*lambda2-1)*(4*lambda2-3)+4*lambda2*(4*lambda2-1)*(4*lambda2 -2))/6;
            return valarray<T>({phi_xi, phi_eta});
        case 2:
            phi_xi = phi_lambda3_xi*((4*lambda3 -1)*(4*lambda3-2)*(4*lambda3-3)+4*lambda3*(4*lambda3-2)*(4*lambda3 -3)
                + 4*lambda3*(4*lambda3-1)*(4*lambda3-3)+4*lambda3*(4*lambda3-1)*(4*lambda3 -2))/6;
            phi_eta = phi_lambda3_eta*((4*lambda3 -1)*(4*lambda3-2)*(4*lambda3-3)+4*lambda3*(4*lambda3-2)*(4*lambda3 -3)
                + 4*lambda3*(4*lambda3-1)*(4*lambda3-3)+4*lambda3*(4*lambda3-1)*(4*lambda3 -2))/6;
            return valarray<T>({phi_xi, phi_eta});
        case 3:
            phi_xi = 8.0/3 *(phi_lambda3_xi * lambda2 *(4*lambda2-1)*(4*lambda2 -2)+ phi_lambda2_xi *lambda3 *
                ((4*lambda2 -1)*(4*lambda2 -2)+4*lambda2 *(4*lambda2 -2)+4*lambda2 *(4*lambda2 -1)));
            phi_eta = 8.0/3 *(phi_lambda3_eta * lambda2 *(4*lambda2-1)*(4*lambda2 -2)+ phi_lambda2_eta *lambda3 *
                ((4*lambda2 -1)*(4*lambda2 -2)+4*lambda2 *(4*lambda2 -2)+4*lambda2 *(4*lambda2 -1)));
            return valarray<T>({phi_xi, phi_eta});
        case 4:
            phi_xi = 4.0 *(phi_lambda2_xi * (lambda3 *(4*lambda2-1)*(4*lambda3 -1)+4*lambda2*lambda3*(4*lambda3-1))+
                phi_lambda3_xi * (lambda2 *(4*lambda2-1)*(4*lambda3 -1)+4*lambda2*lambda3*(4*lambda2-1)));
            phi_eta = 4.0 *(phi_lambda2_eta * (lambda3 *(4*lambda2-1)*(4*lambda3 -1)+4*lambda2*lambda3*(4*lambda3-1))+
                phi_lambda3_eta * (lambda2 *(4*lambda2-1)*(4*lambda3 -1)+4*lambda2*lambda3*(4*lambda2-1)));
            return valarray<T>({phi_xi, phi_eta});
        case 5:
            phi_xi = 8.0/3 *(phi_lambda2_xi * lambda3 *(4*lambda3-1)*(4*lambda3 -2)+ phi_lambda3_xi *lambda2 *
                ((4*lambda3 -1)*(4*lambda3 -2)+4*lambda3 *(4*lambda3 -2)+4*lambda3 *(4*lambda3 -1)));
            phi_eta = 8.0/3 *(phi_lambda2_eta * lambda3 *(4*lambda3-1)*(4*lambda3 -2)+ phi_lambda3_eta *lambda2 *
                ((4*lambda3 -1)*(4*lambda3 -2)+4*lambda3 *(4*lambda3 -2)+4*lambda3 *(4*lambda3 -1)));
            return valarray<T>({phi_xi, phi_eta});
        case 6:
            phi_xi = 8.0/3 *(phi_lambda1_xi * lambda3 *(4*lambda3-1)*(4*lambda3 -2)+ phi_lambda3_xi *lambda1 *
                ((4*lambda3 -1)*(4*lambda3 -2)+4*lambda3 *(4*lambda3 -2)+4*lambda3 *(4*lambda3 -1)));
            phi_eta = 8.0/3 *(phi_lambda1_eta * lambda3 *(4*lambda3-1)*(4*lambda3 -2)+ phi_lambda3_eta *lambda1 *
                ((4*lambda3 -1)*(4*lambda3 -2)+4*lambda3 *(4*lambda3 -2)+4*lambda3 *(4*lambda3 -1)));
            return valarray<T>({phi_xi, phi_eta});
        case 7:
            phi_xi = 4.0 *(phi_lambda1_xi * (lambda3 *(4*lambda1-1)*(4*lambda3 -1)+4*lambda1*lambda3*(4*lambda3-1))+
                phi_lambda3_xi * (lambda1 *(4*lambda1-1)*(4*lambda3 -1)+4*lambda1*lambda3*(4*lambda1-1)));
            phi_eta = 4.0 *(phi_lambda1_eta * (lambda3 *(4*lambda1-1)*(4*lambda3 -1)+4*lambda1*lambda3*(4*lambda3-1))+
                phi_lambda3_eta * (lambda1 *(4*lambda1-1)*(4*lambda3 -1)+4*lambda1*lambda3*(4*lambda1-1)));
            return valarray<T>({phi_xi, phi_eta});
        case 8:
            phi_xi = 8.0/3 *(phi_lambda3_xi * lambda1 *(4*lambda1-1)*(4*lambda1 -2)+ phi_lambda1_xi *lambda3 *
                ((4*lambda1 -1)*(4*lambda1 -2)+4*lambda1 *(4*lambda1 -2)+4*lambda1 *(4*lambda1 -1)));
            phi_xi = 8.0/3 *(phi_lambda3_eta * lambda1 *(4*lambda1-1)*(4*lambda1 -2)+ phi_lambda1_eta *lambda3 *
                ((4*lambda1 -1)*(4*lambda1 -2)+4*lambda1 *(4*lambda1 -2)+4*lambda1 *(4*lambda1 -1)));
            return valarray<T>({phi_xi, phi_eta});
        case 9:
            phi_xi = 8.0/3 *(phi_lambda2_xi * lambda1 *(4*lambda1-1)*(4*lambda1 -2)+ phi_lambda1_xi *lambda2 *
                ((4*lambda1 -1)*(4*lambda1 -2)+4*lambda1 *(4*lambda1 -2)+4*lambda1 *(4*lambda1 -1)));
            phi_xi = 8.0/3 *(phi_lambda2_eta * lambda1 *(4*lambda1-1)*(4*lambda1 -2)+ phi_lambda1_eta *lambda2 *
                ((4*lambda1 -1)*(4*lambda1 -2)+4*lambda1 *(4*lambda1 -2)+4*lambda1 *(4*lambda1 -1)));
            return valarray<T>({phi_xi, phi_eta});
        case 10:
            phi_xi = 4.0 *(phi_lambda1_xi * (lambda2 *(4*lambda1-1)*(4*lambda2 -1)+4*lambda1*lambda2*(4*lambda2-1))+
                phi_lambda2_xi * (lambda1 *(4*lambda1-1)*(4*lambda2 -1)+4*lambda1*lambda2*(4*lambda1-1)));
            phi_eta = 4.0 *(phi_lambda1_eta * (lambda2 *(4*lambda1-1)*(4*lambda2 -1)+4*lambda1*lambda2*(4*lambda2-1))+
                phi_lambda2_eta * (lambda1 *(4*lambda1-1)*(4*lambda2 -1)+4*lambda1*lambda2*(4*lambda1-1)));
            return valarray<T>({phi_xi, phi_eta});
        case 11:
            phi_xi = 8.0/3 *(phi_lambda1_xi * lambda2 *(4*lambda2-1)*(4*lambda2 -2)+ phi_lambda2_xi *lambda1 *
                ((4*lambda2 -1)*(4*lambda2 -2)+4*lambda2 *(4*lambda2 -2)+4*lambda2 *(4*lambda2 -1)));
            phi_eta = 8.0/3 *(phi_lambda1_eta * lambda2 *(4*lambda2-1)*(4*lambda2 -2)+ phi_lambda2_eta *lambda1 *
                ((4*lambda2 -1)*(4*lambda2 -2)+4*lambda2 *(4*lambda2 -2)+4*lambda2 *(4*lambda2 -1)));
            return valarray<T>({phi_xi, phi_eta});
        case 12:
            phi_xi = 32*(phi_lambda1_xi*lambda2*lambda3*(4*lambda3 -1)+ phi_lambda2_xi*lambda1*lambda3*(4*lambda3 -1)+
                phi_lambda3_xi*lambda1*lambda2*(8*lambda3 -1));
            phi_eta = 32*(phi_lambda1_eta*lambda2*lambda3*(4*lambda3 -1)+ phi_lambda2_eta*lambda1*lambda3*(4*lambda3 -1)+
                phi_lambda3_eta*lambda1*lambda2*(8*lambda3 -1));
            return valarray<T>({phi_xi, phi_eta});
        case 13:
            phi_xi = 32*(phi_lambda2_xi*lambda1*lambda3*(4*lambda1 -1)+ phi_lambda3_xi*lambda1*lambda2*(4*lambda1 -1)+
                phi_lambda1_xi*lambda2*lambda3*(8*lambda1 -1));
            phi_eta = 32*(phi_lambda2_eta*lambda1*lambda3*(4*lambda1 -1)+ phi_lambda3_eta*lambda1*lambda2*(4*lambda1 -1)+
                phi_lambda1_eta*lambda2*lambda3*(8*lambda1 -1));
            return valarray<T>({phi_xi, phi_eta});
        case 14:
            phi_xi = 32*(phi_lambda1_xi*lambda2*lambda3*(4*lambda2 -1)+ phi_lambda3_xi*lambda1*lambda2*(4*lambda2-1)+
                phi_lambda2_xi*lambda1*lambda3*(8*lambda2 -1));
            phi_eta = 32*(phi_lambda1_eta*lambda2*lambda3*(4*lambda2 -1)+ phi_lambda3_eta*lambda1*lambda2*(4*lambda2 -1)+
                phi_lambda2_eta*lambda1*lambda3*(8*lambda2 -1));
            return valarray<T>({phi_xi, phi_eta});
        
    }
};